import rclpy
from rclpy.node import Node
from tf2_ros import TransformListener, Buffer#TransformListener, Buffer: 用于接收和缓存 TF（坐标变换）数据
from tf_transformations import euler_from_quaternion#四元数转换

class TFListener(Node):
    def __init__(self):
        super().__init__('tf2_listener')#节点初始化
        self.buffer = Buffer()#创建缓冲区,用于储存接收到的TF数据
        self.listener = TransformListener(self.buffer, self)#创建一个 TF 监听器，把监听到的数据保存在 buffer 中
        self.timer = self.create_timer(1, self.get_transform)#每隔 1 秒调用一次 get_transform() 方法(定时器:想我了么孩子们)

    def get_transform(self):#这里是核心回调函数
        try:
            tf = self.buffer.lookup_transform(
                'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1))
            """
            从 map 坐标系到 base_footprint 坐标系的变换。
            Time(seconds=0) 表示获取最新的可用变换。
            Duration(seconds=1) 是等待变换的最大时间
            """
            transform = tf.transform
            rotation_euler = euler_from_quaternion([
                transform.rotation.x,
                transform.rotation.y,
                transform.rotation.z,
                transform.rotation.w
            ])
            self.get_logger().info(
                f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}')
        except Exception as e:
            self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}')

def main():
    rclpy.init()
    node = TFListener()
    rclpy.spin(node)
    rclpy.shutdown()


if __name__ == '__main__':
    main()